// ************************************
// TODO : TESTER LE CODE ACTUEL AVEC LA POSITION DU ROBOT

// ****************************


#include "fonctions.h"

// **********************************
//  FONCTION DE CONNEXION AU ROBOT
// **********************************
void connecter(void * arg) {

	// *********************
	//     DECLARATIONS
	// *********************
    
    int status;
    int status_robot;
    DMessage *message;
    rt_printf("tconnect : Debut de l'exécution de tconnect\n");
    
    // *********************
    //         CODE
    // *********************

    while (1) {
        rt_printf("tconnect : Attente du sémaphore semConnecterRobot\n");
        rt_sem_p(&semConnecterRobot, TM_INFINITE);
        rt_printf("tconnect : Ouverture de la communication avec le robot\n");
        status = robot->open_device(robot);
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status;
        if (status == STATUS_OK) {
        	//TODO: apres il faudra utiliser le watchdog
            status_robot = robot->start_insecurely(robot);
            //status_robot = robot->start(robot);
            if (status_robot == STATUS_OK){
            	rt_sem_v(&semEtatRobot);
            	rt_printf("tconnect : Robot démarré\n");
            }
            else rt_printf("tconnect : Erreur de demarrage du robot\n");
        }
        rt_mutex_release(&mutexEtat);

        message = d_new_message();
        if (message == NULL) {
            rt_printf("d_new_message : Problème allocation mémoire\n");
        } else {
            message->put_state(message, status);
			// Envoie du status au moniteur
            if (serveur->send(serveur, message) == -1) {
                rt_printf("serveur : Erreur de l'envoie du message...");
            }
        }
    }
}

// **********************************
//  FONCTION DE GESTION COMMUNICATION
// **********************************

//Communication entre le moniteur et le superviseur
void communiquer(void *arg) {

	// *********************
	//     DECLARATIONS
	// *********************

    DMessage *msg = d_new_message();
    int nb_octets_recus = 0;
    int num_msg = 0;
    int status_robot = 0xFF;
    
    DMessage *message;
    DAction *action;
    
    // *********************
    //         CODE
    // *********************

	while(1){
			rt_printf("tserver : Début de l'exécution de serveur\n");
			serveur->open(serveur, "8000");
			rt_printf("tserver : Connexion\n");

			rt_mutex_acquire(&mutexEtat, TM_INFINITE);
			etatCommMoniteur = 0; 												// ???
			rt_mutex_release(&mutexEtat);

		do {
				//rt_printf("tserver : Attente d'un message\n");
				nb_octets_recus = serveur->receive(serveur, msg);
				num_msg++;
				if (nb_octets_recus > 0)
				{
				    switch (msg->get_type(msg))
				    {
				        case MESSAGE_TYPE_ACTION:
				            rt_printf("tserver : Le message %d reçu est une action\n", num_msg);
				            action = d_new_action();
				            action->from_message(action, msg);

				            switch (action->get_order(action))
				            {
								case ACTION_CONNECT_ROBOT:
		                        rt_printf("tserver : Action connecter robot\n");
		                        // Libération du sémaphore pour que le thread "connecter" reprenne la main et connecte le robot
		                        rt_sem_v(&semConnecterRobot);
		                        break;
                            
							case ACTION_FIND_ARENA:
								rt_printf("tserver : Action trouver arène\n");
								 rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
								 etatCamera = ACTION_FIND_ARENA;
								 rt_mutex_release(&mutexEtatCamera);
								break;

							case ACTION_ARENA_IS_FOUND:

								rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
								etatCamera = ACTION_ARENA_IS_FOUND;
								rt_mutex_release(&mutexEtatCamera);
								break;

							case ACTION_ARENA_FAILED:
								rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
								etatCamera = ACTION_ARENA_FAILED;
								rt_mutex_release(&mutexEtatCamera);
								break;

							case ACTION_COMPUTE_CONTINUOUSLY_POSITION:
								rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
								etatCamera = ACTION_COMPUTE_CONTINUOUSLY_POSITION;
								rt_mutex_release(&mutexEtatCamera);
								break;

							case ACTION_STOP_COMPUTE_POSITION:
								rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
								etatCamera = ACTION_STOP_COMPUTE_POSITION;
								rt_mutex_release(&mutexEtatCamera);
								break;
						}

                case MESSAGE_TYPE_MOVEMENT:
                    rt_printf("tserver : Le message reçu %d est un mouvement\n",
                            num_msg);

                    rt_mutex_acquire(&mutexMove, TM_INFINITE);
                    move->from_message(move, msg);
                    move->print(move);
                    rt_mutex_release(&mutexMove);
                    break;

                case MESSAGE_TYPE_MISSION:
                	rt_printf("tserver : Le message %d reçu est une mission\n", num_msg);
					mission->from_message(mission, msg);

					switch (mission -> type)
                	{
						case MISSION_TYPE_REACH_COORDINATE :
							  // Récuperer position objectif
							  mission -> get_position(mission, position_objectif);

							  rt_mutex_acquire(&mutexMission, TM_INFINITE);
							  etatMission = MISSION_RUNNING;
							  rt_mutex_release(&mutexMission);

							  rt_mutex_acquire(&mutexMove, TM_INFINITE);
							  executer_mission(position, position_objectif);
							  rt_mutex_release(&mutexMove);

							  rt_mutex_acquire(&mutexMission, TM_INFINITE);
							  etatMission = MISSION_STOPPED;
							  rt_mutex_release(&mutexMission);

							  msg->mission_terminate(msg, mission->get_id(mission));
							  serveur -> send(serveur, msg);
							  break;


							  case MISSION_TYPE_STOP :
								  rt_mutex_acquire(&mutexMission, TM_INFINITE);
								  etatMission = MISSION_STOPPED;
								  rt_mutex_release(&mutexMission);

								  msg->mission_terminate(msg, mission->get_id(mission));
								  serveur -> send(serveur, msg);
								  break;
                	}
				}
			}

			rt_mutex_acquire(&mutexEtat, TM_INFINITE);
			status_robot = etatCommRobot;
			rt_mutex_release(&mutexEtat);

			if(status_robot!=STATUS_OK){
				message = d_new_message();
				if (message == NULL) {
					rt_printf("d_new_message : Problème allocation mémoire\n");
				} else {
					message->put_state(message, status_robot);
					// Envoie du status au moniteur
					if (serveur->send(serveur, message) == -1) {
						rt_printf("serveur : Erreur de l'envoie du message...");
					}
				}
			}

		} while (nb_octets_recus > 0);
		
		rt_printf("tserver : Fermeture de connexion\n");
    	serveur->close(serveur);
    	rt_mutex_acquire(&mutexMove, TM_INFINITE);
        move->set(move, DIRECTION_STOP, 0);
        move->print(move);
        rt_mutex_release(&mutexMove);
    }
}
// **********************************
//  FONCTION DE DEPLACEMENT DU ROBOT
// **********************************

// Entre superviseur et robot
void deplacer(void *arg) {

	// *********************
	//     DECLARATIONS
	// *********************

    int status = 0xFF;
    int compteur_fail = 0;
    DMessage *message;


    rt_printf("tmove : Debut de l'éxecution de periodique à 200ms\n");
	// Période fixée à 200ms
    rt_task_set_periodic(NULL, TM_NOW, 200000000);
    
    // *********************
    //         CODE
    // *********************
    
    while (1) {

        /* Attente de l'activation périodique */
        rt_task_wait_period(NULL);
        rt_printf("tmove : Activation périodique\n");
        rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        status = etatCommRobot;
        rt_mutex_release(&mutexEtat);
        if (status == STATUS_OK) {
            rt_mutex_acquire(&mutexMove, TM_INFINITE);


            switch (move->get_direction(move)) {
                case DIRECTION_FORWARD:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_LEFT:
                    status = robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_AVANT_LENT);
                    break;
                case DIRECTION_RIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_ARRIERE_LENT);
                    break;
                case DIRECTION_STOP:
                    status = robot->set_motors(robot, MOTEUR_STOP, MOTEUR_STOP);
                    break;
                case DIRECTION_STRAIGHT:
                    status = robot->set_motors(robot, MOTEUR_AVANT_LENT, MOTEUR_AVANT_LENT);
                    break;
            }
            rt_mutex_release(&mutexMove);
            if (status != STATUS_OK) {
                compteur_fail++;
// A tester : (tester avec compteur_fail > 0)
                if (compteur_fail > 2) {
                	compteur_fail = 0;
                	rt_mutex_acquire(&mutexEtat, TM_INFINITE);
		            etatCommRobot = status;
		            rt_mutex_release(&mutexEtat);

		            message = d_new_message();
		            if (message == NULL) {
		                rt_printf("d_new_message : Problème allocation mémoire\n");
		            } else {
		                message->put_state(message, status);
		    			// On prévient le moniteur que la connexion avec le robot a été perdue
		                if (serveur->send(serveur, message) == -1) {
		                    rt_printf("serveur : Erreur de l'envoie du message...");
		                }
		            }
                }
            } else {
            	compteur_fail = 0;
            	//robot->reload_wdt(robot);
            	//rt_printf("Watchdog rechargé\n");
            }
        }
    }
}


// **********************************
//  FONCTION DE GESTION DE LA BATTERIE
// **********************************

//Etat de la batterie (periodique 1s)
void gestion_batterie(void *arg)
{
	int status_robot = 0xFF;
	int status_batterie = -1;
	DMessage *message;
 	DBattery *batterie = d_new_battery();
	
	if (batterie == NULL) {
         rt_printf("d_new_batterie : Problème allocation mémoire\n");
    }
	
	// Période fixée à 1s
    rt_task_set_periodic(NULL, TM_NOW, 1000000000);
    rt_sem_p(&semEtatRobot, TM_INFINITE);
    
    while (1) {
        //Attente de l'activation périodique
        rt_task_wait_period(NULL);
        rt_printf("tEtat : Activation périodique\n");

		status_robot = robot->get_vbat(robot, &status_batterie);
		rt_mutex_acquire(&mutexEtat, TM_INFINITE);
        etatCommRobot = status_robot;
        rt_mutex_release(&mutexEtat);

        // Pas la peine d'envoyer le message si l'état est inconnu
		if (status_batterie != -1){
			message = d_new_message();
			if (message == NULL) {
				rt_printf("d_new_message : Problème allocation mémoire\n");
			} else {

				batterie->set_level(batterie, status_batterie);
				message->put_battery_level(message, batterie);
				// Envoie du status au moniteur
				if (serveur->send(serveur, message) == -1) {
					rt_printf("serveur : Erreur de l'envoie du message...");
				}
			}
		}
    }
}

// **********************************
//  FONCTION DE RECUP IMAGES WEBCAM
// **********************************
void webcam(void *arg)
{
	// *********************
	//     DECLARATIONS
	// *********************
 
	DMessage * msg = d_new_message();

	// *********************
    //         CODE
    // *********************

	rt_printf("tcamera : Debut de l'éxecution la fonction webcam \n");
	
	// Création d'un accès à la webcam
	rt_printf("tcamera : Création d'un accès à la webcam \n");
	camera -> open(camera);
	camera -> print(camera);
	
	rt_task_set_periodic(NULL, TM_NOW, 600000000);
	
	while (1) {

	  	if(etatCommMoniteur == STATUS_OK)
	  	{


			/* Attente de l'activation périodique */
			rt_task_wait_period(NULL);
			//rt_printf("tmove : Activation périodique\n");

			rt_mutex_acquire(&mutexCamera, TM_INFINITE);

			// Si l'utilisateur a demandé une détection d'arène, on execute la fonction

			switch (etatCamera) {

				case ACTION_FIND_ARENA :
					detecter_arene();
					break;

				case WAIT_ARENA_ANSWER :
					 sleep(1);
					 break;

				case ACTION_COMPUTE_CONTINUOUSLY_POSITION :
					detecter_robot();
					break;

				default :

					// Récupération de l'image de la webcam
					//rt_printf("tcamera : Récupération de l'image de la webcam \n");
					camera -> get_frame(camera, img);
	
					// Compression de l'image en JPEG
					//rt_printf("tcamera : Compression de l'image en JPEG \n");
					jpegimg -> compress(jpegimg, img);
		
					// Encapsulation de l'image dans un message
					//rt_printf("tcamera : Encapsulation de l'image dans un message \n");
					msg -> put_jpeg_image(msg, jpegimg);

					// Envoi de l'image
					//rt_printf("tcamera : Envoi de l'image \n");
					serveur -> send(serveur, msg);
			}

			rt_mutex_release(&mutexCamera);
		}
	  	else {
	  		sleep(1);
	  	}
	}
}

// **********************************
//  FONCTION DE DETECTION DE L'ARENE
// **********************************
void detecter_arene() {

	// *********************
	//     DECLARATIONS
	// *********************
	DMessage * msg = d_new_message();

    // *********************
    //         CODE
    // *********************

	// Récupération de l'image de la webcam
	camera -> get_frame(camera, img);
	
	// Détection de l'arène
	arena = img -> compute_arena_position(img);
	
	// Traçage du contour de l'arène
	d_imageshop_draw_arena(img, arena);
	
	// Compression de l'image en JPEG
	jpegimg -> compress(jpegimg, img);
	
	// Encapsulation de l'image dans un message
	msg -> put_jpeg_image(msg, jpegimg);

	// Envoi de l'image 
	serveur -> send(serveur, msg);
	
	// On place la caméra dans un état d'attente de réponse
	rt_mutex_acquire(&mutexEtatCamera, TM_INFINITE);
	etatCamera = WAIT_ARENA_ANSWER; 
	rt_mutex_release(&mutexEtatCamera);
}

// **********************************
//  FONCTION DE DETECTION DU ROBOT
// **********************************
void detecter_robot() {

	// *********************
	//     DECLARATIONS
	// *********************
	DMessage * msg = d_new_message();

    // *********************
    //         CODE
    // *********************
    
    //rt_printf("detecter_robot : Debut de la detection du robot \n");

	// Récupération de l'image de la webcam
	camera -> get_frame(camera, img);
	
   //Si l'arène est nulle, la fonction appelée gère ceci en cherchant sur toute l'image
  position = img -> compute_robot_position(img, arena);
	
	if(position != NULL)
	{
		// Traçage du contour de l'arène
		d_imageshop_draw_position(img, position);
	}
	else {
		position = d_new_position();
		position -> set(position, 0, 0, 0);
	}
	
	jpegimg -> compress(jpegimg, img);
	
	// Position et direction numériques sur le moniteur
	msg -> put_position(msg, position);
	
	// Envoi de la position
	serveur -> send(serveur, msg);

	// Encapsulation de l'image dans un message
	msg -> put_jpeg_image(msg, jpegimg);
	
	// Envoi de l'image 
	serveur -> send(serveur, msg);
}

// ********************************************
//  FONCTION DE CALCUL DE LA DIRECTION DU ROBOT
// ********************************************

int executer_mission(DPosition* p_robot, DPosition* cible) {

	// *********************
	//     DECLARATIONS
	// *********************

	float X_cible = cible->get_x(cible);
	float Y_cible = cible->get_y(cible);
	float X_robot = p_robot->get_x(p_robot);
	float Y_robot = p_robot->get_y(p_robot);
   
  //rapports entre la taille de l'arene sur l'image et dans la réalité
  float rapport_X = 0.0;  
  float rapport_Y = 0.0;
  
  //distances réelles en millimètres
  float distance_X_MM = 0.0;
  float distance_Y_MM = 0.0;

	rt_printf("robot = (%f, %f) \n", X_robot, Y_robot);
	rt_printf("cible = (%f, %f) \n", X_cible, Y_cible);

	float X_diff = abs(X_cible - X_robot);
	float Y_diff = abs(Y_cible - Y_robot);

	float angle_rotation = -1;
	float angle_robot = p_robot->get_orientation(p_robot)* 180.0 / M_PI;
  
  int angle_deplacement = 0; //angle duquel le robot doit tourner pour faire face à son objectif
  int distance_deplacement = 0;  //distance que le robot doit parcourir pour atteindre son objectif

	int sens_rotation = -1;

	DMessage * msg = d_new_message();

    // *********************
    //         CODE
    // *********************

	if(X_cible > X_robot)
	{
		if(Y_cible < Y_robot)
		{
			angle_rotation = 270.0 + atan(X_diff / Y_diff) * 180.0 / M_PI;
		}
		else if(Y_cible > Y_robot)
		{
			angle_rotation = atan(Y_diff / X_diff) * 180.0 / M_PI;
		}
		else {
			angle_rotation = 0.0;
		}
	}
	else if(X_cible < X_robot)
	{
		if(Y_cible < Y_robot)
		{
			angle_rotation = 180.0 + atan(Y_diff / X_diff) * 180.0 / M_PI;
		}
		else if(Y_cible > Y_robot)
		{
			angle_rotation = 90.0 + atan(X_diff / Y_diff) * 180.0 / M_PI;
		}
		else {
			angle_rotation = 180.0;
		}
	}
	else {
		if(Y_cible < Y_robot)
		{
			angle_rotation = 90.0;
		}
		else if(Y_cible > Y_robot)
		{
			angle_rotation = 270.0;
		}
		else {
			rt_printf("MISSION_TYPE_STOP \n");
		}
	}

	rt_printf("angle rotation = %f \n", angle_rotation);
	rt_printf("angle robot = %f \n", angle_robot);

	angle_deplacement = angle_robot - angle_rotation;

	rt_printf("angle deplacement avant calcul = %d \n", angle_deplacement);


	if(angle_deplacement > 0)
	{
		if(angle_deplacement > 180)
		{
			//DEPLACEMENT VERS LA DROITE
			rt_printf("droite > 180 \n");


			angle_deplacement = 360 - angle_deplacement;

			sens_rotation = HORAIRE;
		}
		else {
			//DEPLACEMENT VERS LA GAUCHE
			rt_printf("gauche < 180 \n");

			sens_rotation = ANTI_HORAIRE;
		}
	}
	else {
		if(angle_deplacement < -180)
			{
				//DEPLACEMENT VERS LA GAUCHE
				rt_printf("gauche < -180 \n");

				angle_deplacement = 360 + angle_deplacement;

				sens_rotation = ANTI_HORAIRE;
			}
			else {
				//DEPLACEMENT VERS LA DROITE
				rt_printf("droite > -180 \n");

				angle_deplacement = abs(angle_deplacement);

				sens_rotation = HORAIRE;
			}
	}

  if(arena != NULL)
  {
     rapport_X = 800.0 / arena->get_width(arena);
     rapport_Y = 600.0 / arena->get_height(arena);
  }
  // sinon les calculs sont effectués avec valeurs proches
  else {
     rapport_X = 800.0 / 500.0;
     rapport_Y = 600.0 / 350.0;
  }
  
	distance_X_MM = X_diff * rapport_X;
	distance_Y_MM = Y_diff * rapport_Y;

	distance_deplacement = sqrt(distance_X_MM * distance_X_MM + distance_Y_MM * distance_Y_MM);

	rt_printf("angle deplacement = %d \n", angle_deplacement);

	if(distance_deplacement > 30)
	{
		// Rotation
		if (angle_deplacement > 3)
		{
			while(robot->turn(robot, angle_deplacement, sens_rotation) != STATUS_OK);

			wait_robot_busy();
		}

		// Avancer
		while(robot->set_motors(robot, MOTEUR_ARRIERE_LENT, MOTEUR_ARRIERE_LENT) != STATUS_OK);

		while(robot->move(robot, distance_deplacement) != STATUS_OK);

		wait_robot_busy();

	}

	msg->mission_terminate(msg, mission->get_id(mission));
	serveur -> send(serveur, msg);
}

void wait_robot_busy()
{
	int busy = 0;
	int mission_state = -1;

	do {
		while(robot->is_busy(robot, &busy) != STATUS_OK);

		rt_mutex_acquire(&mutexMission, TM_INFINITE);
		mission_state = etatMission;
		rt_mutex_release(&mutexMission);

	} while(busy != 0 && etatMission == MISSION_RUNNING);
}
